#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>

#include "debug.h"
#include "os_api.h"
#include "ps_api.h"
#include "i2c.h"
#include "hal/gpio.h"
#include "uart.h"
#include "debug.h"

typedef struct{
    int port;   // 0:uart, 1:uart3, 2:uart4
    int len;
    UINT8 *data;
}uartParam;

static OSMsgQRef uartMsgQ = NULL;

#define UART_TASK_STACK_SIZE     1024*8
static void* uart_task_stack = NULL;
static OSTaskRef uart_task_ref = NULL;
static int gps_data_to_uart = 0;
static void uart_thread(void *param);


#define GSENSOR_TASK_STACK_SIZE     1024*2
static void* gsensor_task_stack = NULL;
static OSTaskRef gsensor_task_ref = NULL;

static void gsensor_thread(void *param);


#define LED_WD_TASK_STACK_SIZE     1024*2
static void* led_wd_task_stack = NULL;
static OSTaskRef led_wd_task_ref = NULL;

static int stopWDG = 0;

static void led_wd_thread(void *param);


// 设备主串口
void mainUartRecvCallback(UINT8 * recv_data, UINT32 recv_len)
{	
	uartParam uart_data = {0};
    OSA_STATUS osa_status;
    
    char *tempbuf = (char *)malloc(recv_len+1);
    memset(tempbuf, 0x0, recv_len+1);
    memcpy(tempbuf, (char *)recv_data, recv_len);
	
    LOG_PRINTF("%s[%d]: recv_len:%d, recv_data:%s\n", __FUNCTION__, __LINE__, recv_len, (char *)(tempbuf)); 
    uart_data.data = (UINT8 *)tempbuf;
    uart_data.len = recv_len;
    uart_data.port = 0;
	
    osa_status = OSAMsgQSend(uartMsgQ, sizeof(uartParam), (UINT8*)&uart_data, OSA_NO_SUSPEND);
    ASSERT(osa_status == OS_SUCCESS);
}

// GPS对应串口
//void uart3RecvCallback(UINT8 * recv_data, UINT32 recv_len)
void uart4RecvCallback(UINT8 * recv_data, UINT32 recv_len)
{	
	uartParam uart_data = {0};
    OSA_STATUS osa_status;
    
    char *tempbuf = (char *)malloc(recv_len+1);
    memset(tempbuf, 0x0, recv_len+1);
    memcpy(tempbuf, (char *)recv_data, recv_len);
	
    //LOG_PRINTF("%s[%d]: recv_len:%d, recv_data:%s\n", __FUNCTION__, __LINE__, recv_len, (char *)(tempbuf)); 
    uart_data.data = (UINT8 *)tempbuf;
    uart_data.len = recv_len;
    uart_data.port = 2;

    osa_status = OSAMsgQSend(uartMsgQ, sizeof(uartParam), (UINT8*)&uart_data, OSA_NO_SUSPEND);
    ASSERT(osa_status == OS_SUCCESS);
}


#define GNSS_SENTENCE_LENGTH	200

static char gnss_RMC_Sentence[GNSS_SENTENCE_LENGTH] = {0};
static char gnss_GGA_Sentence[GNSS_SENTENCE_LENGTH] = {0};
static char gnss_VTG_Sentence[GNSS_SENTENCE_LENGTH] = {0};
static char gnss_GSA_Sentence[GNSS_SENTENCE_LENGTH] = {0};
static char gnss_GLL_Sentence[GNSS_SENTENCE_LENGTH] = {0};

void gnss_process_nmea_sting(char *data)
{
	int i;
	static int tempLen = 0;
	static char tempSentence[GNSS_SENTENCE_LENGTH]= {0};

	//LOG_PRINTF("gnss_process_nmea_sting : %s\n", data);
	
	for (i = 0; i < strlen(data); i++){
        if (tempLen >= sizeof(tempSentence)) {
            memset(tempSentence, 0x0, GNSS_SENTENCE_LENGTH);
            tempLen = 0;
        }
		if (*(data+i) == 0x0D){
			tempSentence[tempLen++] = *(data+i);
		}else if (*(data+i) == 0x0A){
			tempSentence[tempLen++] = *(data+i);
			LOG_PRINTF("tempSentence : %s", tempSentence);

			if (!strncmp(tempSentence, "$GNGGA", strlen("$GNGGA"))){
				memset(gnss_GGA_Sentence, 0x0, GNSS_SENTENCE_LENGTH);
				memcpy(gnss_GGA_Sentence, tempSentence, strlen(tempSentence));
			}else if (!strncmp(tempSentence, "$GNGSA", strlen("$GNGSA"))){
				memset(gnss_GSA_Sentence, 0x0, GNSS_SENTENCE_LENGTH);
				memcpy(gnss_GSA_Sentence, tempSentence, strlen(tempSentence));
			}else if (!strncmp(tempSentence, "$GPGSV", strlen("$GPGSV"))){

			}else if (!strncmp(tempSentence, "$GNRMC", strlen("$GNRMC"))){
				memset(gnss_RMC_Sentence, 0x0, GNSS_SENTENCE_LENGTH);
				memcpy(gnss_RMC_Sentence, tempSentence, strlen(tempSentence));				
			}else if (!strncmp(tempSentence, "$GPVTG", strlen("$GPVTG"))){
				memset(gnss_VTG_Sentence, 0x0, GNSS_SENTENCE_LENGTH);
				memcpy(gnss_VTG_Sentence, tempSentence, strlen(tempSentence));
			}else if (!strncmp(tempSentence, "$GNGLL", strlen("$GNGLL"))){
				memset(gnss_GLL_Sentence, 0x0, GNSS_SENTENCE_LENGTH);
				memcpy(gnss_GLL_Sentence, tempSentence, strlen(tempSentence));
			}else{
				
			}
	
			memset(tempSentence, 0x0, GNSS_SENTENCE_LENGTH);
			tempLen = 0;
		}else{
			tempSentence[tempLen++] = *(data+i);
		}
	}
}


#if 0
#define GPIO_RS485RE		13
#define GPIO_GPS_EN			15
#define GPIO_WATCHDOG		05

#define GPIO_LED_NET_STATUS		34
#define GPIO_LED_STATUS			70
#else
#define GPIO_RS485RE		17
#define GPIO_GPS_EN			18
#define GPIO_WATCHDOG		124

#define GPIO_LED_NET_STATUS		21
#define GPIO_LED_STATUS			12
#endif

void watchdog_kill(void)
{
	GPIOReturnCode gpio_status = GPIORC_OK;		
	int value = hal_GpioIn(GPIO_WATCHDOG) == GPIORC_HIGH ? 0 : 1;
	
	gpio_status = hal_GpioSetDir(GPIO_WATCHDOG, GPIO_OUT_PIN);
	if (gpio_status != GPIORC_OK){		
		LOG_PRINTF("hal_GpioSetDir status: 0x%lx", gpio_status);
	}	
	
	LOG_PRINTF("watchdog_kill value: 0x%lx", value);
	
	gpio_status = hal_GpioOut(GPIO_WATCHDOG, value);
	if (gpio_status != GPIORC_OK){
		LOG_PRINTF("status: 0x%lx", gpio_status);
	}
}

void led_on(int ledgpio)
{
	GPIOReturnCode gpio_status = GPIORC_OK;	

	if (hal_GpioIn(ledgpio) == GPIORC_HIGH){		
		return;
	}
	
	gpio_status = hal_GpioSetDir(ledgpio, GPIO_OUT_PIN);
	if (gpio_status != GPIORC_OK){
		
		LOG_PRINTF("hal_GpioSetDir status: 0x%lx", gpio_status);
	}	
	gpio_status = hal_GpioOut(ledgpio, 1);
	if (gpio_status != GPIORC_OK){
		LOG_PRINTF("hal_GpioOut status: 0x%lx", gpio_status);
	}			
}

void led_off(int ledgpio)
{
	GPIOReturnCode gpio_status = GPIORC_OK;	

	if (hal_GpioIn(ledgpio) == GPIORC_LOW){		
		return;
	}
	
	gpio_status = hal_GpioSetDir(ledgpio, GPIO_OUT_PIN);
	if (gpio_status != GPIORC_OK){
		
		LOG_PRINTF("GpioSetDirection status: 0x%lx", gpio_status);
	}	
	gpio_status = hal_GpioOut(ledgpio, 0);
	if (gpio_status != GPIORC_OK){
		LOG_PRINTF("GpioSetLevel status: 0x%lx", gpio_status);
	}			
}


void gps_poweron(void)
{
	GPIOReturnCode gpio_status = GPIORC_OK;	
	
	gpio_status = hal_GpioSetDir(GPIO_GPS_EN, GPIO_OUT_PIN);
	if (gpio_status != GPIORC_OK){
		
		LOG_PRINTF("hal_GpioSetDir status: 0x%lx", gpio_status);
	}	
	gpio_status = hal_GpioOut(GPIO_GPS_EN, 1);
	if (gpio_status != GPIORC_OK){
		LOG_PRINTF("hal_GpioOut status: 0x%lx", gpio_status);
	}			
}

void gps_poweroff(void)
{
	GPIOReturnCode gpio_status = GPIORC_OK;	
	
	gpio_status = hal_GpioSetDir(GPIO_GPS_EN, GPIO_OUT_PIN);
	if (gpio_status != GPIORC_OK){
		
		LOG_PRINTF("hal_GpioSetDir status: 0x%lx", gpio_status);
	}	
	gpio_status = hal_GpioOut(GPIO_GPS_EN, 0);
	if (gpio_status != GPIORC_OK){
		LOG_PRINTF("hal_GpioOut status: 0x%lx", gpio_status);
	}			
}

void rs485_set_send_state(void)
{
	GPIOReturnCode gpio_status = GPIORC_OK;	
	
	gpio_status = hal_GpioSetDir(GPIO_RS485RE, GPIO_OUT_PIN);
	if (gpio_status != GPIORC_OK){
		
		LOG_PRINTF("hal_GpioSetDir status: 0x%lx", gpio_status);
	}	
	gpio_status = hal_GpioOut(GPIO_RS485RE, 0);	// 485切换到发送
	if (gpio_status != GPIORC_OK){
		LOG_PRINTF("hal_GpioOut status: 0x%lx", gpio_status);
	}
}

void rs485_set_recv_state(void)
{
	GPIOReturnCode gpio_status = GPIORC_OK;	
	
	gpio_status = hal_GpioSetDir(GPIO_RS485RE, GPIO_OUT_PIN);
	if (gpio_status != GPIORC_OK){
		
		LOG_PRINTF("hal_GpioSetDir status: 0x%lx", gpio_status);
	}	
	gpio_status = hal_GpioOut(GPIO_RS485RE, 1);	// 485切换到接收
	if (gpio_status != GPIORC_OK){
		LOG_PRINTF("hal_GpioOut status: 0x%lx", gpio_status);
	}			
}


static void uart_thread(void *param)
{
	int ret = 0;
    OSA_STATUS status;	
    uartParam uart_data = {0};

	GPIOReturnCode gpio_status = GPIORC_OK;
	
    // uart uart4 默认波特率都是115200
	// 打开主串口
	hal_UartOpenCb(UART_PORT_2, 9600, mainUartRecvCallback);
	
	// 打开GPS串口
    //hal_UartEnableGpio(UART_PORT_3, 2, 3);	
    //hal_UartOpenCb(UART_PORT_3, 9600, uart3RecvCallback);
	hal_UartOpenCb(UART_PORT_4, 9600, uart4RecvCallback);

	gps_poweron();
	
	rs485_set_recv_state();// 485切换到接收
	
	rs485_set_send_state();// 485切换到发送
	hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
	hal_UartWaitFifoEmpty(UART_PORT_2, 50);
	rs485_set_recv_state();// 485切换到接收	

	OSATaskSleep(100);

	rs485_set_send_state();// 485切换到发送
	hal_UartWrite(UART_PORT_2, "\r\nOK111\r\n",9);
	hal_UartWaitFifoEmpty(UART_PORT_2, 50);
	rs485_set_recv_state();// 485切换到接收	

    while (1) {
        memset(&uart_data, 0x00, sizeof(uartParam));        
        status = OSAMsgQRecv(uartMsgQ, (UINT8 *)&uart_data, sizeof(uartParam), OSA_SUSPEND);    //recv data from uart and uart4
		ASSERT(status == OS_SUCCESS);

		if (uart_data.data) {
			LOG_PRINTF("%s: port %d len:%d\n", __FUNCTION__, uart_data.port, uart_data.len);
			
			if (uart_data.port == 0){
				
				static int isTestMode = 0;
				if (isTestMode == 0){
				
					if (!strncmp((char *)uart_data.data, "AT+ENTERTRTEST=1\r\n", strlen("AT+ENTERTRTEST=1\r\n"))
						||!strncmp((char *)uart_data.data, "AT+ENTERTRTEST\r\n", strlen("AT+ENTERTRTEST\r\n"))){	
						isTestMode = 1;
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收	
					}else if (!strncmp((char *)uart_data.data, "AT+ENTERTRTEST?\r\n", strlen("AT+ENTERTRTEST?\r\n"))){	
						rs485_set_send_state();// 485切换到发送
						if (isTestMode)
							hal_UartWrite(UART_PORT_2, "\r\n+ENTERTRTEST: 1\r\nOK\r\n", strlen("\r\n+ENTERTRTEST: 1\r\nOK\r\n"));
						else
							hal_UartWrite(UART_PORT_2, "\r\n+ENTERTRTEST: 0\r\nOK\r\n", strlen("\r\n+ENTERTRTEST: 0\r\nOK\r\n"));
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收							
					}else{
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "***",3);
						hal_UartWrite(UART_PORT_2, uart_data.data, uart_data.len);
						hal_UartWrite(UART_PORT_2, "***\r\n",5);
						LOG_PRINTF("hal_UartWaitFifoEmpty");
						ret = hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						LOG_PRINTF("hal_UartWaitFifoEmpty ret: %d", ret);
						rs485_set_recv_state();// 485切换到接收	
					}					
				}else{
					if (!strncmp((char *)uart_data.data, "AT+ENTERTRTEST=0\r\n", strlen("AT+ENTERTRTEST=0\r\n"))){	
						isTestMode = 0;
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收
					}else if (!strncmp((char *)uart_data.data, "AT+ENTERTRTEST?\r\n", strlen("AT+ENTERTRTEST?\r\n"))){	
						rs485_set_send_state();// 485切换到发送
						if (isTestMode)
							hal_UartWrite(UART_PORT_2, "\r\n+ENTERTRTEST: 1\r\nOK\r\n", strlen("\r\n+ENTERTRTEST: 1\r\nOK\r\n"));
						else
							hal_UartWrite(UART_PORT_2, "\r\n+ENTERTRTEST: 0\r\nOK\r\n", strlen("\r\n+ENTERTRTEST: 0\r\nOK\r\n"));
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收								
					}else if (!strncmp((char *)uart_data.data, "AT+CGPS=1\r\n", strlen("AT+CGPS=1\r\n"))){						
						gps_poweron();
						gps_data_to_uart = 1;
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收					
					}else if(!strncmp((char *)uart_data.data, "AT+CGPS=0\r\n", strlen("AT+CGPS=0\r\n"))){						
						gps_poweroff();
						gps_data_to_uart = 0;
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收	
					}else if(!strncmp((char *)uart_data.data, "AT+WATCHDOG=1\r\n", strlen("AT+WATCHDOG=1\r\n"))){						
						stopWDG = 0;
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收
					}else if(!strncmp((char *)uart_data.data, "AT+WATCHDOG=0\r\n", strlen("AT+WATCHDOG=0\r\n"))){						
						stopWDG = 1;
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收	
					}else if(!strncmp((char *)uart_data.data, "AT+RS485TEST\r\n", strlen("AT+RS485TEST\r\n"))){						
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收		
					}else if(!strncmp((char *)uart_data.data, "AT+RS232TEST\r\n", strlen("AT+RS232TEST\r\n"))){						
						rs485_set_send_state();// 485切换到发送
						hal_UartWrite(UART_PORT_2, "\r\nOK\r\n",6);
						hal_UartWaitFifoEmpty(UART_PORT_2, 50);
						rs485_set_recv_state();// 485切换到接收
					}						
				}
				
			}else if (uart_data.port == 2){				
				//LOG_PRINTF("%s", (char *)(uart_data.data));
				gnss_process_nmea_sting((char *)(uart_data.data));
				if (gps_data_to_uart){					
					rs485_set_send_state();// 485切换到发送
					hal_UartWrite(UART_PORT_2, uart_data.data, uart_data.len);					
					hal_UartWaitFifoEmpty(UART_PORT_2, 50);
					rs485_set_recv_state();// 485切换到接收		
				}
			}

			free(uart_data.data);
		}
  
    }
}

#define I2C_SLAVE_ADDR      0x18

static int i2c_no = 0;		//i2c1 (SCL/SDA: GPIO_49/GPIO_50)

#define ACC_RANGE_2           0x3     //acceleration measurement range ±2g
#define ACC_RANGE_4           0x5	  //acceleration measurement range ±4g
#define ACC_RANGE_8           0x8	  //acceleration measurement range ±8g

#define ACC_RANGE_2_UNIT      (3.91)
#define ACC_RANGE_4_UNIT      (7.81)
#define ACC_RANGE_8_UNIT      (15.63)

#define ACC_RANGE_2_UNIT_53      (0.98)
#define ACC_RANGE_4_UNIT_53      (1.95)
#define ACC_RANGE_8_UNIT_53      (3.91)

#define ACC_SPEED_LOCAL       (9.8) //Local gravitational acceleration, default 9.8


void STK8BA50_Initialization(void)
{
	int ret;
	unsigned char RegAddr, RegData;
	//---------------------
	//Reset
	//---------------------
	//Register 0x14กXReset Register
	//D7            D6          D5          D4          D3          D2          D1          D0
	//
	//Writing 0xB6 to this Register will reset IC
	//
	RegAddr = 0x14;
	RegData = 0xB6;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);


	RegAddr = 0x11;
	RegData = 0x80;		//SUSPEND
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);			
	//---------------
	//Set Mode as interrupt and measurement operation mode
	//---------------
	//0x1A: INT Mapping
	//D7         D6        D5           D4              D3          D2              D1              D0
	//int2_data  Reserved  Reserved     Reserved        Reserved    Reserved        Reserved        int1_data
	//
	//Map new data interrupt to INT1
	//
	RegAddr = 0x1A;
	RegData = 0x01;	
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	

	//0x17: INT Setting
	//D7          D6        D5          D4          D3          D2           D1          D0
	//Reserved    Reserved  Reserved    data_en     low_en      high_en_z    high_en_y   high_en_x
	//
	//Enable new data interrupt.
	//

	RegAddr = 0x17;
	RegData = 0x10;	
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	

	//0x21: INT Latch Setting
	//D7          D6        D5          D4          D3              D2              D1              D0
	//reset_int   Reserved  Reserved    Reserved    latch_int<3>    latch_int<2>    latch_int<1>    latch_int<0>
	//
	//Set INT pin as non-latched mode
	//

	RegAddr = 0x21;
	RegData = 0x00;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	


	//0x1E: INT Source Selection
	//D7         D6        D5              D4             D3          D2              D1            D0
	//Reserved   Reserved  int_src_data    int_src_tap    Reserved    int_src_slope   int_src_high  int_src_low
	//
	//Filtered data source is used for new data interrupt.
	//

	RegAddr = 0x13;
	RegData = 0x00;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	


	//0x20: INT PP-OD, Active High/Low
	//D7         D6        D5           D4              D3          D2              D1              D0
	//Reserved   Reserved  Reserved     Reserved        int2_od     int2_|v|        int1_od         int1_|v|
	//
	//Set INT as Push-Pull, Active-High
	//


	RegAddr = 0x20;
	RegData = 0x01;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	

	//---------------
	//Set Measure Range, Resolution
	//---------------
	//0x0F: Range Selection (Read/Write)
	//D7             D6           D5           D4           D3          D2          D1          D0
	//Reserved       Reserved     Reserved     Reserved     Range<3>    Range<2>    Range<1>    Range<0>
	// *** RNG[3:0]     MEASUREMENT RANGE   ***
	//     0011           กำ2 g
	//     0101           กำ4 g
	//     1000           กำ8 g
	//     1100           กำ16 g

	RegAddr = 0x0F;
	RegData = 0x03;	// range = 2g
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	

	//---------------
	//Output Data Rate Settings :
	//---------------
	//0x10: Bandwidth
	//D7            D6          D5          D4      D3      D2      D1      D0
	//Reserved      Reserved    Reserved    bw<4>   bw<3>   bw<2>   bw<1>   bw<0>

	RegAddr = 0x10;
	RegData = 0x0C;	 // ODR=125Hz
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	
	//---------------
	//I2C WatchDog
	//---------------
	//0x34: I2C WatchDog
	//D7        D6          D5          D4          D3          D2          D1              D0
	//Reserved  Reserved    Reserved    Reserved    Reserved    i2c_wdt_en  i2c_wdt_sel     spi3
	//
	//Enable I2C WatchDog
	//
	RegAddr = 0x34;
	RegData = 0x04;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);			

	RegAddr = 0x11;   //set Register Adrress
	RegData = 0x40 | (0x0E << 1);//low power sleep 500ms
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);				
}

void STK8BA53_Initialization(void)
{
	int ret;
	unsigned char RegAddr, RegData;
	//---------------------
	//Reset
	//---------------------
	//Register 0x14กXReset Register
	//D7            D6          D5          D4          D3          D2          D1          D0
	//
	//Writing 0xB6 to this Register will reset IC
	//
	RegAddr = 0x14;
	RegData = 0xB6;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);


	RegAddr = 0x11;
	RegData = 0x80;		//SUSPEND
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);			
	//---------------
	//Set Mode as interrupt and measurement operation mode
	//---------------
	//0x1A: INT Mapping
	//D7         D6        D5           D4              D3          D2              D1              D0
	//int2_data  Reserved  Reserved     Reserved        Reserved    Reserved        Reserved        int1_data
	//
	//Map new data interrupt to INT1
	//
	RegAddr = 0x1A;
	RegData = 0x01;	
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	

	//0x17: INT Setting
	//D7          D6        D5          D4          D3          D2           D1          D0
	//Reserved    Reserved  Reserved    data_en     low_en      high_en_z    high_en_y   high_en_x
	//
	//Enable new data interrupt.
	//

	RegAddr = 0x17;
	RegData = 0x10;	
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	

	//0x21: INT Latch Setting
	//D7          D6        D5          D4          D3              D2              D1              D0
	//reset_int   Reserved  Reserved    Reserved    latch_int<3>    latch_int<2>    latch_int<1>    latch_int<0>
	//
	//Set INT pin as non-latched mode
	//

	RegAddr = 0x21;
	RegData = 0x00;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	


	//0x1E: INT Source Selection
	//D7         D6        D5              D4             D3          D2              D1            D0
	//Reserved   Reserved  int_src_data    int_src_tap    Reserved    int_src_slope   int_src_high  int_src_low
	//
	//Filtered data source is used for new data interrupt.
	//

	RegAddr = 0x13;
	RegData = 0x00;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	


	//0x20: INT PP-OD, Active High/Low
	//D7         D6        D5           D4              D3          D2              D1              D0
	//Reserved   Reserved  Reserved     Reserved        int2_od     int2_|v|        int1_od         int1_|v|
	//
	//Set INT as Push-Pull, Active-High
	//


	RegAddr = 0x20;
	RegData = 0x01;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	

	//---------------
	//Set Measure Range, Resolution
	//---------------
	//0x0F: Range Selection (Read/Write)
	//D7             D6           D5           D4           D3          D2          D1          D0
	//Reserved       Reserved     Reserved     Reserved     Range<3>    Range<2>    Range<1>    Range<0>
	// *** RNG[3:0]     MEASUREMENT RANGE   ***
	//     0011           กำ2 g
	//     0101           กำ4 g
	//     1000           กำ8 g
	//     1100           กำ16 g

	RegAddr = 0x0F;
	RegData = 0x03;	// range = 2g
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	

	//---------------
	//Output Data Rate Settings :
	//---------------
	//0x10: Bandwidth
	//D7            D6          D5          D4      D3      D2      D1      D0
	//Reserved      Reserved    Reserved    bw<4>   bw<3>   bw<2>   bw<1>   bw<0>

	RegAddr = 0x10;
	RegData = 0x0C;	 // ODR=125Hz
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);	
	//---------------
	//I2C WatchDog
	//---------------
	//0x34: I2C WatchDog
	//D7        D6          D5          D4          D3          D2          D1              D0
	//Reserved  Reserved    Reserved    Reserved    Reserved    i2c_wdt_en  i2c_wdt_sel     spi3
	//
	//Enable I2C WatchDog
	//
	RegAddr = 0x34;
	RegData = 0x04;
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);			

	RegAddr = 0x11;   //set Register Adrress
	RegData = 0x40 | (0x0E << 1);//low power sleep 500ms
	ret = hal_I2cWriteEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &RegData, 1);	
	LOG_PRINTF("%s,%d ret 0x%x\n", __FUNCTION__, __LINE__, ret);				
}

static void gsensor_thread(void *param)
{
	int ret;
	unsigned char RegAddr = 0;
	unsigned char RegData = 0;
	unsigned char chipid = 0;
    UINT8 value[7],x,y,z;
	float data[3];
	int temperature = 0;
	int v;
	
	ret = hal_I2cInit(i2c_no, STANDARD_MODE);	
	LOG_PRINTF("hal_I2cInit ret %d", ret);	
	
	while(1){
		RegAddr = 0x00;
		ret = hal_I2cReadEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, &chipid, 1);
        if(0 == ret)
		{
			LOG_PRINTF("chipid: 0x%lx", chipid);
            break;
        }		
		OSATaskSleep(200);
	}
	
	if (chipid == 0x86){
		STK8BA50_Initialization();		

		while (1) {

			RegAddr = 0x02;
			ret = hal_I2cReadEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, (unsigned char *)value, sizeof(value));	
			
			LOG_PRINTF("%s,%d ret %d, RegData: 0x%02X,0x%02X,0x%02X,0x%02X,0x%02X,0x%02X,0x%02X\n", __FUNCTION__, __LINE__, ret, 
									value[0], value[1], value[2], value[3], value[4], value[5], value[6]);		
			
			v = ((signed char)value[1] << 2) | (value[0] >> 6);
			data[0] = (float)((double)v * ACC_RANGE_2_UNIT /1000 * ACC_SPEED_LOCAL);
			x = value[0] & 0x01;
			LOG_PRINTF("stk8ba50_reg_read value x %d\n", v);
			
			v = ((signed char)value[3] << 2) | (value[2] >> 6);
			data[1] = (float)((double)v * ACC_RANGE_2_UNIT /1000 * ACC_SPEED_LOCAL);
			y = value[2] & 0x01;
			LOG_PRINTF("stk8ba50_reg_read value y %d\n", v);
			
			v = ((signed char)value[5] << 2) | (value[4] >> 6);
			data[2] = (float)((double)v * ACC_RANGE_2_UNIT /1000 * ACC_SPEED_LOCAL);
			z = value[4] & 0x01;
			LOG_PRINTF("stk8ba50_reg_read value z %d\n", v);
			//v = (signed char)value[6];
			//temperature = (v / 2) + 24;
			LOG_PRINTF("APP-INFO acc: %6.3f %6.3f %6.3f\n", data[0], data[1], data[2]);
			if(!(x || y || z)){
				LOG_PRINTF("x:%u,y:%u.z:%u\n", x,y,z);        
			}		
			
			OSATaskSleep(200);
		}		
	}else if (chipid == 0x87){
		STK8BA53_Initialization();
		
		while (1) {

			RegAddr = 0x02;
			ret = hal_I2cReadEx(i2c_no, I2C_SLAVE_ADDR, &RegAddr, 1, (unsigned char *)value, sizeof(value));	
			
			LOG_PRINTF("%s,%d ret %d, RegData: 0x%02X,0x%02X,0x%02X,0x%02X,0x%02X,0x%02X,0x%02X\n", __FUNCTION__, __LINE__, ret, 
									value[0], value[1], value[2], value[3], value[4], value[5], value[6]);	
									
			v = ((signed char)value[1] << 4) | (value[0] >> 4);
			data[0] = (float)((double)v * ACC_RANGE_2_UNIT /1000 * ACC_SPEED_LOCAL);
			x = value[0] & 0x01;
			LOG_PRINTF("stk8ba53_reg_read value x %d\n", v);
			
			v = ((signed char)value[3] << 4) | (value[2] >> 4);
			data[1] = (float)((double)v * ACC_RANGE_2_UNIT /1000 * ACC_SPEED_LOCAL);
			y = value[2] & 0x01;
			LOG_PRINTF("stk8ba53_reg_read value y %d\n", v);
			
			v = ((signed char)value[5] << 4) | (value[4] >> 4);
			data[2] = (float)((double)v * ACC_RANGE_2_UNIT /1000 * ACC_SPEED_LOCAL);
			z = value[4] & 0x01;
			LOG_PRINTF("stk8ba53_reg_read value z %d\n", v);
			
			LOG_PRINTF("APP-INFO acc: %6.3f %6.3f %6.3f\n", data[0], data[1], data[2]);
			
			if(!(x || y || z))
			{
				LOG_PRINTF("x:%u,y:%u.z:%u\n", x,y,z);        
            }
			
			OSATaskSleep(200);
        }	
	}
}


static void led_wd_thread(void *param)
{
	int t_dly = 0;
	
	UINT32 lastWDGTick = 0;
	
    while (1) {
		if (ps_GetNetworkReady()){
			
			led_on(GPIO_LED_STATUS);  //HIGH
			
			OSATaskSleep(40);	// 200ms
			if (t_dly < 9)
				led_on(GPIO_LED_NET_STATUS);	// 1800ms high
			else
				led_off(GPIO_LED_NET_STATUS);	// 200ms low

			if (++t_dly >= 10)
				t_dly = 0;
		} else {
			led_on(GPIO_LED_STATUS);  //LOW
			
			OSATaskSleep(100);	// 500ms
			if (t_dly < 1)
				led_on(GPIO_LED_NET_STATUS);	// 500ms high
			else
				led_off(GPIO_LED_NET_STATUS);	// 500ms low

			if (++t_dly >= 10)
				t_dly = 0;			
		}
				
		// 60s
		if (!stopWDG && (lastWDGTick == 0 || (OSAGetTicks()-lastWDGTick) > 60*200)){
			watchdog_kill();
			lastWDGTick = OSAGetTicks();
		}		
    }
}


int main(void)
{
    OSA_STATUS status;

    LOG_PRINTF("%s[%d]: starting...\n", __FUNCTION__, __LINE__);
	
    status = OSAMsgQCreate(&uartMsgQ, "uartMsgQ", sizeof(uartParam), 300, OS_FIFO);
    ASSERT(status == OS_SUCCESS);

    uart_task_stack = malloc(UART_TASK_STACK_SIZE);
    ASSERT(uart_task_stack != NULL);
    status = OSATaskCreate(&uart_task_ref, uart_task_stack, UART_TASK_STACK_SIZE, 82, "uart_thread", uart_thread, NULL);
    ASSERT(status == OS_SUCCESS);	

    gsensor_task_stack = malloc(GSENSOR_TASK_STACK_SIZE);
    ASSERT(gsensor_task_stack != NULL);
    status = OSATaskCreate(&gsensor_task_ref, gsensor_task_stack, GSENSOR_TASK_STACK_SIZE, 83, "gsensor_thread", gsensor_thread, NULL);
    ASSERT(status == OS_SUCCESS);
	
    led_wd_task_stack = malloc(LED_WD_TASK_STACK_SIZE);
    ASSERT(led_wd_task_stack != NULL);
    status = OSATaskCreate(&led_wd_task_ref, led_wd_task_stack, LED_WD_TASK_STACK_SIZE, 84, "led_wd_thread", led_wd_thread, NULL);
    ASSERT(status == OS_SUCCESS);	

	return 0;
}